#include <std_msgs/Int32.h>

#include <boost/foreach.hpp>

#include <matchmaker/advanced_client.h>

/**
 * @brief Example of advanced querying, monitoring and using a service
 */
class ExampleAdvConsumer
{
public:
  ExampleAdvConsumer() { }
  ~ExampleAdvConsumer() { }

  void main()
  {
    // Create the query with a callback. AdvancedClient then ensures that
    // we receive a callback whenever the query results change.
    myServiceQuery_ = mmClient_.queryService("MadeUpService",
        boost::bind(&ExampleAdvConsumer::serviceQueryCB, this, _1));

    if (!myServiceQuery_) {
      ROS_ERROR_STREAM("Failed to create service query!");
      return;
    }

    // Spin to process ROS events, until killed.
    ros::spin();
  }

private:
  void serviceQueryCB(const matchmaker::ServiceQuery& query)
  {
    // Don't do anything if we have a monitored service already.
    if (myServiceMon_) return;

    const matchmaker::L_ServiceDetails* serviceList = query.getResults();

    BOOST_FOREACH(const matchmaker::ServiceDetailsPtr& service, (*serviceList)) {
      if (service->status.isUsable()) {
        ROS_INFO_STREAM("Subscribing to " << service->name << " on topic " <<  service->topic);
        myServiceSub_ = nh_.subscribe(service->topic, 1, &ExampleAdvConsumer::myServiceMsgCB, this);

        // Set up a service status monitor, with a callback function.
        myServiceMon_ = mmClient_.monitorService(service,
            boost::bind(&ExampleAdvConsumer::myServiceStatusCB, this, _1));

        return;
      }
    }

    ROS_INFO_STREAM("Waiting for usable service.");
  }

  void myServiceStatusCB(const matchmaker::ServiceDetails& service)
  {
    ROS_INFO_STREAM("Service " << service.name << " status " << service.status.toString());

    if (!service.status.isUsable()) {
      ROS_WARN_STREAM("Lost service " << service.name << " on " << service.topic);
      myServiceMon_.shutdown();
      myServiceSub_.shutdown();
    }
  }

  void myServiceMsgCB(std_msgs::Int32ConstPtr msg)
  {
    ROS_INFO_STREAM("Received msg value " << msg->data << " on " << myServiceSub_.getTopic());
  }

  ros::NodeHandle nh_;

  matchmaker::AdvancedClient mmClient_;

  matchmaker::ServiceQuery myServiceQuery_;
  matchmaker::ServiceMonitor myServiceMon_;
  ros::Subscriber myServiceSub_;
};

int main(int argc, char ** argv)
{
  // AnonymousName init option adds changeable number to node name, so
  // we can run multiple instances of this node at once without clashes.
  ros::init(argc, argv, "adv_consumer", ros::init_options::AnonymousName);

  ExampleAdvConsumer example;
  example.main();
  exit(0);
}

/** \example example_adv_consumer.cpp
 * This code illustrates how to query for, and monitor a simple ROS message
 * publishing service service using the matchmaker::AdvancedClient.
 * You can find it within the matchmaker package, in the file
 * src/example_adv_consumer.cpp.
 * Once you have built the matchmaker module, you can try running it like so:
\verbatim
$ rosrun matchmaker example_adv_consumer
\endverbatim
 *
 * In contrast to the polled findService approach of the simpler
 * example_consumer, this code uses the queryService method to create
 * a ServiceQuery object that performs a user callback whenever the query
 * results change. The query can also return several matching services,
 * whereas the findService method in the simpler matchmaker::Client will
 * only return the first service that matches.
 */
